/*!
  @file feature_extractor.h
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#ifndef _FEATURE_EXTRACTOR_H_
#define _FEATURE_EXTRACTOR_H_

#include <boost/scoped_ptr.hpp>
#include <boost/shared_ptr.hpp>

#include <opencv2/core/core.hpp>

#include "mrover_vo/feature.h"
#include "mrover_vo/triangulation.h"
#include "mrover_vo/validity_checker-impl.h"

namespace vo {

/*! 
 *  @brief  Feature extractor
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 *  detailed documentaiton for this class
 */

class FeatureExtractor {
  typedef typename Feature::Ptr FeaturePtr;
  typedef boost::shared_ptr<Feature::KeyPoints> KeyPointsPtr;
  typedef boost::shared_ptr<Feature::Descriptors> DescriptorsPtr;

 public:
  typedef boost::scoped_ptr<FeatureExtractor> Ptr;


  FeatureExtractor() 
      : detector_(cv::FeatureDetector::create("GridHARRIS"))
      , descriptor_(cv::DescriptorExtractor::create("BRIEF"))
      , matcher_(new FeatureMatcher)
      , triangulation_(new Triangulation)
      , img_vec_(2)
  { 
    boost::shared_ptr<ValidityCheckerForStereoImages> valid_chk(new ValidityCheckerForStereoImages);
    matcher_->addValidityCheck(valid_chk);
  }

  ~FeatureExtractor() { }


  void compute();
  inline void compute(FeaturePtr &feature) {
    setOutputFeature(feature);
    compute();
  }

  void setImages(const std::vector<cv::Mat> &images) { img_vec_ = images; }
  void setMask(const cv::Mat &mask) { mask_ = mask; }
  void setOutputFeature(FeaturePtr &feature) { feature_ = feature; }
  FeaturePtr getFeature() const { return feature_; }

  void setCameraParams(const CameraParams::ConstPtr &cp) { triangulation_->setCameraParams(cp); }

 private:
  bool initCompute() {
    if (!img_vec_[0].data || !img_vec_[1].data || !feature_) return false;
    return true;
  }

  void detectSubpixel();
  void removeUnpairedFeatures();


  cv::Ptr<cv::FeatureDetector> detector_;
  cv::Ptr<cv::DescriptorExtractor> descriptor_;
  FeatureMatcher::Ptr matcher_;
  Triangulation::Ptr triangulation_;

  std::vector<cv::Mat> img_vec_;
  cv::Mat mask_;
  FeaturePtr feature_;
};

} // namespace vo

#endif
